#Copyright Deng（灯哥） (ream_d@yeah.net)  Py-apple dog project
#Github:https://github.com/ToanTech/py-apple-quadruped-robot
#Licensed under the Apache License, Version 2.0 (the "License");
from math import sin,cos,pi,atan,tan

# PID参数设置
KP_S=0.02    # 扭矩-舵机角速度P环
KP_X=0.4     # X坐标P环
KP_Z=0.4     # Z坐标P环 
KD_X=0.0     # X坐标D环
KD_Z=0.0     # Z坐标D环

# 全局变量初始化
leg1_X_N=0; leg2_X_N=0; leg3_X_N=0; leg4_X_N=0
leg1_Z_N=100; leg2_Z_N=100; leg3_Z_N=100; leg4_Z_N=100
leg1_shoulder_origin=0; leg2_shoulder_origin=0; leg3_shoulder_origin=0; leg4_shoulder_origin=0
leg1_ham_origin=130.5188; leg2_ham_origin=130.5188; leg3_ham_origin=130.5188; leg4_ham_origin=130.5188  
leg1_shank_origin=-91.7463; leg2_shank_origin=-91.7463; leg3_shank_origin=-91.7463; leg4_shank_origin=-91.7463
leg1_X_last=leg1_X_N; leg2_X_last=leg2_X_N; leg3_X_last=leg3_X_N; leg4_X_last=leg4_X_N
leg1_Z_last=leg1_Z_N; leg2_Z_last=leg2_Z_N; leg3_Z_last=leg3_Z_N; leg4_Z_last=leg4_Z_N
leg1_Y_N=0; leg2_Y_N=0; leg3_Y_N=0; leg4_Y_N=0
leg1_Y_last=0; leg2_Y_last=0; leg3_Y_last=0; leg4_Y_last=0

def cal_vmc_single_leg(l1, l2, l3, shoulder_origin, ham_origin, shank_origin, Xe, Ye, Ze, X, Y, Z, Xl, Yl, Zl, is_left=True):
    # 参数验证
    if not all(isinstance(v, (int, float)) for v in [Xe, Ye, Ze, X, Y, Z, Xl, Yl, Zl]):
        raise ValueError("坐标参数必须是数字类型")
    
    # PD控制器 (修正坐标系)
    Vx = X - Xl
    Vy = Y - Yl
    Vz = Z - Zl
    Fx = KP_X*(Xe-X) + KD_X*(0-Vx)
    Fy = KP_Z*(Ye-Y) + KD_Z*(0-Vy)
    Fz = KP_Z*(Ze-Z) + KD_Z*(0-Vz)
    
    # 角度转弧度 (增加角度限制)
    shoulder = max(-90, min(90, shoulder_origin)) * pi/180
    ham = max(0, min(180, ham_origin)) * pi/180
    shank = max(-90, min(0, shank_origin)) * pi/180
    
    # 修正力矩计算 (肩膀改为绕X轴旋转)
    tshoulder = -Fz * l3/2 * (1 if is_left else -1)  # 绕X轴力矩
    tham = Fx*(l1*cos(ham) + l2*cos(ham+shank)) - Fz*(l1*sin(ham) + l2*sin(ham+shank))
    tshank = Fx*l2*cos(ham+shank) - Fz*l2*sin(ham+shank)
    
    # 更新关节角度 (带限制)
    shoulder_new = max(-90, min(90, shoulder_origin + tshoulder * KP_S))
    ham_new = max(0, min(180, ham_origin + tham * KP_S))
    shank_new = max(-90, min(0, shank_origin + tshank * KP_S))
    
    # 正运动学计算 (修正坐标系)
    x_base = (l3/2) * (1 if is_left else -1)  # Changed l0 to l3
    x1 = x_base + l1 * cos(ham) * cos(shoulder)
    y1 = l1 * cos(ham) * sin(shoulder)
    z1 = -l1 * sin(ham)
    
    x2 = x1 + l2 * cos(ham+shank) * cos(shoulder)
    y2 = y1 + l2 * cos(ham+shank) * sin(shoulder)
    z2 = z1 - l2 * sin(ham+shank)
    
    return tshoulder*KP_S, tham*KP_S, tshank*KP_S, x2, y2, z2, X, Z

def cal_v(case, l1, l2, l3, x1, x2, x3, x4, y1, y2, y3, y4, z1, z2, z3, z4):
    global leg1_shoulder_origin, leg2_shoulder_origin, leg3_shoulder_origin, leg4_shoulder_origin
    global leg1_ham_origin, leg2_ham_origin, leg3_ham_origin, leg4_ham_origin
    global leg1_shank_origin, leg2_shank_origin, leg3_shank_origin, leg4_shank_origin
    global leg1_X_N, leg2_X_N, leg3_X_N, leg4_X_N
    global leg1_Z_N, leg2_Z_N, leg3_Z_N, leg4_Z_N
    global leg1_X_last, leg2_X_last, leg3_X_last, leg4_X_last
    global leg1_Z_last, leg2_Z_last, leg3_Z_last, leg4_Z_last
    
    try:
        # 调用单腿计算 (参数顺序调整为l1,l2,l3)
        A_1 = cal_vmc_single_leg(l1, l2, l3, leg1_shoulder_origin, leg1_ham_origin, leg1_shank_origin,
                           x1, y1, z1, leg1_X_N, leg1_Y_N, leg1_Z_N, leg1_X_last, leg1_Y_last, leg1_Z_last, True)
        A_2 = cal_vmc_single_leg(l1, l2, l3, leg2_shoulder_origin, leg2_ham_origin, leg2_shank_origin,
                           x2, y2, z2, leg2_X_N, leg2_Y_N, leg2_Z_N, leg2_X_last, leg2_Y_last, leg2_Z_last, False)
        A_3 = cal_vmc_single_leg(l1, l2, l3, leg3_shoulder_origin, leg3_ham_origin, leg3_shank_origin,
                           x3, y3, z3, leg3_X_N, leg3_Y_N, leg3_Z_N, leg3_X_last, leg3_Y_last, leg3_Z_last, True)
        A_4 = cal_vmc_single_leg(l1, l2, l3, leg4_shoulder_origin, leg4_ham_origin, leg4_shank_origin,
                           x4, y4, z4, leg4_X_N, leg4_Y_N, leg4_Z_N, leg4_X_last, leg4_Y_last, leg4_Z_last, False)
    except Exception as e:
        print(f"cal_v:{e}")
    # 更新关节角度
    leg1_shoulder_origin += A_1[0]; leg1_ham_origin += A_1[1]; leg1_shank_origin += A_1[2]
    leg2_shoulder_origin += A_2[0]; leg2_ham_origin += A_2[1]; leg2_shank_origin += A_2[2]
    leg3_shoulder_origin += A_3[0]; leg3_ham_origin += A_3[1]; leg3_shank_origin += A_3[2]
    leg4_shoulder_origin += A_4[0]; leg4_ham_origin += A_4[1]; leg4_shank_origin += A_4[2]
    
    # 更新足端位置
    leg1_X_N, leg1_Z_N = A_1[3], A_1[5]
    leg2_X_N, leg2_Z_N = A_2[3], A_2[5] 
    leg3_X_N, leg3_Z_N = A_3[3], A_3[5]
    leg4_X_N, leg4_Z_N = A_4[3], A_4[5]
    
    # 记录上一时刻位置
    leg1_X_last, leg1_Z_last = A_1[6], A_1[7]
    leg2_X_last, leg2_Z_last = A_2[6], A_2[7]
    leg3_X_last, leg3_Z_last = A_3[6], A_3[7]
    leg4_X_last, leg4_Z_last = A_4[6], A_4[7]
    
    # 返回12个舵机角度
    return (
        leg1_shoulder_origin, 180-leg1_ham_origin, -leg1_shank_origin,
        leg2_shoulder_origin, 180-leg2_ham_origin, -leg2_shank_origin,
        leg3_shoulder_origin, 180-leg3_ham_origin, -leg3_shank_origin,
        leg4_shoulder_origin, 180-leg4_ham_origin, -leg4_shank_origin
    )







